/*
 * Sensors.h
 *
 * This class provides an abstraction layer for reading data from the sensors
 *
 */

#ifndef SENSORS_H_
#define SENSORS_H_
#include <iostream>
#include <fstream>
#include "rrcon.h"
#include "init_systems.h"
#include "log.h"

using namespace kthrobot;
namespace Group5 {
    /*
     * THE CALIBRATION DATA EXPECTED TO BE IN A STREAM WITH THE FOLLOWING FORMAT:
     * S B
     * NOTE THAT ALL THESE DATAS SHOULD BE INTEGER.
     */
    class Sensors {

        class IR {
            // Class to get data from the IR sensors
            public:
                void IR(int i, int k, int m_prime, int b_prime)
                {
                    index _= i < 0?0:i > 15 ? 15:i; // Index must be in [0,15]
                    k_ = k;
                    m_prime_ = m_prime;
                    b_prime_ = b_prime;
                }
                int get_distance()
                {
                    int V = kthrobot::RRCon::GetADCData().channel[index_];
                    return ir_calibration_function_(V, k_, m_prime_, b_prime_);
                }
            private:
                const int index_, k_, m_prime_, b_prime_;
                static int ir_calibration_function_(int V, int k, int m_prime, int b_prime);
        };

        public:
        static IR ir_front;
        static IR ir_back;

        static IR ir_right_front;
        static IR ir_right_back;

        static IR ir_left_front;
        static IR ir_left_back;

        static void Init();
    };

}

#endif /* SENSORS_H_ */
